#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/Int32.h>
#include <serial/serial.h>

// 定义机器人的参数
const double wheel_radius = 0.05;  // 轮子半径（米）
const double wheel_base = 0.3;     // 两轮之间的距离（米）
const int ticks_per_revolution = 4096;  // 每转一圈的编码器脉冲数

// 全局变量
double x = 0.0;
double y = 0.0;
double th = 0.0;

// 串口对象
serial::Serial ser;

// 函数声明
void leftEncoderCallback(const std_msgs::Int32::ConstPtr& msg, ros::NodeHandle& nh);
void rightEncoderCallback(const std_msgs::Int32::ConstPtr& msg, ros::NodeHandle& nh);
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, ros::NodeHandle& nh);
void updatePose(double delta_left, double delta_right, ros::NodeHandle& nh);

int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "base_controller_node");
    ros::NodeHandle nh;

    // 打开串口
    // try {
    //     ser.setPort("/dev/ttyUSB0");  // 根据实际情况修改端口号
    //     ser.setBaudrate(115200);      // 根据实际情况修改波特率
    //     serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    //     ser.setTimeout(to);
    //     ser.open();
    // } catch (serial::IOException& e) {
    //     ROS_ERROR_STREAM("Unable to open port ");
    //     return -1;
    // }

    // if (ser.isOpen()) {
    //     ROS_INFO_STREAM("Serial Port initialized");
    // } else {
    //     return -1;
    // }

    // 订阅编码器数据
    ros::Subscriber left_encoder_sub = nh.subscribe<std_msgs::Int32>("left_encoder", 10, 
        std::bind(leftEncoderCallback, std::placeholders::_1, std::ref(nh)));
    ros::Subscriber right_encoder_sub = nh.subscribe<std_msgs::Int32>("right_encoder", 10,
        std::bind(rightEncoderCallback, std::placeholders::_1, std::ref(nh)));

    // 订阅cmd_vel话题
    ros::Subscriber cmd_vel_sub = nh.subscribe<geometry_msgs::Twist>("cmd_vel", 10,
        std::bind(cmdVelCallback, std::placeholders::_1, std::ref(nh)));

    // 进入ROS主循环
    ros::spin();

    return 0;
}

void leftEncoderCallback(const std_msgs::Int32::ConstPtr& msg, ros::NodeHandle& nh) {
    static int last_left_ticks = 0;
    int current_left_ticks = msg->data;
    int delta_left_ticks = current_left_ticks - last_left_ticks;
    last_left_ticks = current_left_ticks;

    double delta_left_distance = (2 * M_PI * wheel_radius * delta_left_ticks) / ticks_per_revolution;

    // 更新位姿
    updatePose(delta_left_distance, 0.0, nh);
}

void rightEncoderCallback(const std_msgs::Int32::ConstPtr& msg, ros::NodeHandle& nh) {
    static int last_right_ticks = 0;
    int current_right_ticks = msg->data;
    int delta_right_ticks = current_right_ticks - last_right_ticks;
    last_right_ticks = current_right_ticks;

    double delta_right_distance = (2 * M_PI * wheel_radius * delta_right_ticks) / ticks_per_revolution;

    // 更新位姿
    updatePose(0.0, delta_right_distance, nh);
}

void updatePose(double delta_left, double delta_right, ros::NodeHandle& nh) {
    // 计算线速度和角速度
    double linear_velocity = (delta_left + delta_right) / 2.0;
    double angular_velocity = (delta_right - delta_left) / wheel_base;

    // 计算时间间隔
    static ros::Time last_time = ros::Time::now();
    ros::Time current_time = ros::Time::now();
    double dt = (current_time - last_time).toSec();
    last_time = current_time;

    // 更新位姿
    double delta_x = (linear_velocity * cos(th)) * dt;
    double delta_y = (linear_velocity * sin(th)) * dt;
    double delta_th = angular_velocity * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

    // 发布里程计信息
    static ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("encode_odom", 10);
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    // 设置位置
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(th);

    // 设置速度
    odom.child_frame_id = "base_footprint";
    odom.twist.twist.linear.x = linear_velocity;
    odom.twist.twist.angular.z = angular_velocity;

    // 发布消息
    odom_pub.publish(odom);

    //ROS_INFO("sour dev x:%.2f y:%.2f", x, y);
}

void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, ros::NodeHandle& nh) {
    // 解析cmd_vel消息
    double linear = msg->linear.x;
    double angular = msg->angular.z;

    // 将速度转换为电机控制信号
    int left_speed = (linear - (angular * wheel_base / 2.0)) / wheel_radius;
    int right_speed = (linear + (angular * wheel_base / 2.0)) / wheel_radius;

    // 构造串口命令
    std::string command = "L" + std::to_string(left_speed) + "R" + std::to_string(right_speed) + "\n";

    if (!ser.isOpen()) {
        return;
    }
    // 通过串口发送命令
    ser.write(command);
}